namespace il
{

class LSS
{
public:
	LSS()
	{

	}
	
	LSS( Point p, Mat im )
	{
		this->compute ( p, im );
	}
		
	Mat_<float> operator()(void)
	{
		return this->d;
	}

	Mat_<float> operator()(Point p, Mat im )
	{
		return this->compute(p,im);
	}
	
	Mat_<float> compute( Point p, Mat input )
	{
		
		if( p.x-odim/2 < 0 || p.y-odim/2 < 0 || p.x+odim/2 >= input.cols || p.y+odim/2 >= input.rows)
			throw Exception( 1 , "point too close to border" , "compute_descriptor" , "lss.h" , 22 );
		
		Mat_<Vec3b> image(input);
		Mat_<Vec3b> ipatch = image( Range(p.y-idim/2,p.y+idim/2), Range(p.x-idim/2, p.x+idim/2)  );
		Mat_<Vec3b> opatch = image( Range(p.y-odim/2,p.y+odim/2), Range(p.x-odim/2, p.x+odim/2)  );
		
		Mat_<Vec3b> w;
		Mat_<float> distance_surface( odim/idim, odim/idim );
		Mat_<float> polar;
		
		float epsilon, eta, corr, current;
		float* angle = new float[angle_steps];
		float* radii = new float[radii_steps];
		
		for (int i = 0 ; i < angle_steps ; ++i)
			angle[i] = i * 2. * CV_PI/angle_steps;
		for(int i  = 0 ; i < radii_steps ; ++i)
			radii[i] = i*(odim/2)/radii_steps;

		for(int i = 0, ii=0 ; i < odim ; i += idim, ++ii)
		{
			for(int j = 0, jj = 0 ; j < odim ; j += idim, ++jj)
			{
				w = opatch(Range(i,i+idim),Range(j,j+idim));
				distance_surface(ii,jj) = ssd(ipatch,w);
			}
		}
		
		//cv::normalize
		normalize( distance_surface, distance_surface, 0., 1., NORM_MINMAX );
		
		//log-polar
		int n, m;
		polar = Mat_<float>::zeros( radii_steps, angle_steps );
		for(int i = 0 ; i < distance_surface.rows ; ++i)
		{
			for(int j = 0 ; j < distance_surface.cols ; ++j)
			{
				epsilon = sqrt( double(i*i+j*j) );
				if(i)
					eta = atan( j/double(i) );
				else
					eta = CV_PI/2.;
		
				n = 0; m = 0;
				while(radii[n++]<epsilon && n < radii_steps){}
				while(angle[m++]<eta && m < angle_steps){}
				--n;--m;
				corr = exp(-distance_surface(i,j)/(varnoise*.1));
				current = polar(n,m);
		
				polar(n,m) += corr;
			}
		}
		//cv::normalize polar
		normalize( polar, polar, 0., 1., NORM_MINMAX );
		this->d = polar;
		return polar;
	}

	void visualize()
	{
		visualize( this->d );
	}

	void visualize( Mat_<float> polar )
	{
		int M = 20;
		Mat_<float> canvas;

		resize( polar, canvas, cvSize( polar.rows*M, polar.cols*M ), 0, 0, INTER_AREA );
		imshow( "LSS Descriptor" , canvas.t() );

		waitKey(10);
	}
	
	static const int idim = 5;
	static const int odim = 80;
	static const int radii_steps = 40;
	static const int angle_steps = 10;
	static const int varnoise = 1;

protected:
	Mat_<float> d;//descriptor
	float ssd( Mat_<Vec3b> a, Mat_<Vec3b> b )
	{
		float c = 0;
		for(int i = 0 ; i<a.rows ; ++i)
			for(int j = 0 ; j<a.cols ; ++j)
				c += ( a(i,j)-b(i,j) ).dot( a(i,j)-b(i,j) );
		return c;
	}

};

int test_LSS()
{
	LSS lss;
	Mat image = imread("baby.jpg");
	imshow( "input", image );
	for(int i = lss.odim ; i < image.rows - lss.odim; i += 10)
	{
		Mat_<float> descriptor = lss(Point(i,i), image);
		lss.visualize(descriptor);
	}	
	return 0;
}
//	class XSelfSimDescriptor
//	{
//	public:
//		XSelfSimDescriptor();
//		XSelfSimDescriptor(int _ssize, int _lsize,
//			int _startDistanceBucket=DEFAULT_START_DISTANCE_BUCKET,
//			int _numberOfDistanceBuckets=DEFAULT_NUM_DISTANCE_BUCKETS,
//			int _nangles=DEFAULT_NUM_ANGLES);
//		XSelfSimDescriptor(const SelfSimDescriptor& ss);
//		virtual ~XSelfSimDescriptor();
//		XSelfSimDescriptor& operator = (const XSelfSimDescriptor& ss);
//
//		size_t getDescriptorSize() const;
//		Size getGridSize( Size imgsize, Size winStride ) const;
//
//		virtual void compute(const Mat& img, vector<float>& descriptors, Size winStride=Size(),
//							 const vector<Point>& locations=vector<Point>()) const;
//		virtual void computeLogPolarMapping(Mat& mappingMask) const;
//		virtual void SSD(const Mat& img, Point pt, Mat& ssd) const;
//
//		int smallSize;
//		int largeSize;
//		int startDistanceBucket;
//		int numberOfDistanceBuckets;
//		int numberOfAngles;
//
//		enum { DEFAULT_SMALL_SIZE = 5, DEFAULT_LARGE_SIZE = 41,
//			DEFAULT_NUM_ANGLES = 20, DEFAULT_START_DISTANCE_BUCKET = 3,
//			DEFAULT_NUM_DISTANCE_BUCKETS = 7 };
//	};
//
//	XSelfSimDescriptor::XSelfSimDescriptor()
//	{
//		smallSize = DEFAULT_SMALL_SIZE;
//		largeSize = DEFAULT_LARGE_SIZE;
//		numberOfAngles = DEFAULT_NUM_ANGLES;
//		startDistanceBucket = DEFAULT_START_DISTANCE_BUCKET;
//		numberOfDistanceBuckets = DEFAULT_NUM_DISTANCE_BUCKETS;
//	}
//
//	XSelfSimDescriptor::XSelfSimDescriptor(int _ssize, int _lsize,
//										 int _startDistanceBucket,
//										 int _numberOfDistanceBuckets, int _numberOfAngles)
//	{
//		smallSize = _ssize;
//		largeSize = _lsize;
//		startDistanceBucket = _startDistanceBucket;
//		numberOfDistanceBuckets = _numberOfDistanceBuckets;
//		numberOfAngles = _numberOfAngles;
//	}
//
//	XSelfSimDescriptor::XSelfSimDescriptor(const SelfSimDescriptor& ss)
//	{
//		smallSize = ss.smallSize;
//		largeSize = ss.largeSize;
//		startDistanceBucket = ss.startDistanceBucket;
//		numberOfDistanceBuckets = ss.numberOfDistanceBuckets;
//		numberOfAngles = ss.numberOfAngles;
//	}
//
//	XSelfSimDescriptor::~XSelfSimDescriptor()
//	{
//	}
//
//	XSelfSimDescriptor& XSelfSimDescriptor::operator = (const XSelfSimDescriptor& ss)
//	{
//		if( this != &ss )
//		{
//			smallSize = ss.smallSize;
//			largeSize = ss.largeSize;
//			startDistanceBucket = ss.startDistanceBucket;
//			numberOfDistanceBuckets = ss.numberOfDistanceBuckets;
//			numberOfAngles = ss.numberOfAngles;
//		}
//		return *this;
//	}
//
//	size_t XSelfSimDescriptor::getDescriptorSize() const
//	{
//		return numberOfAngles*(numberOfDistanceBuckets - startDistanceBucket);
//	}
//
//	Size XSelfSimDescriptor::getGridSize( Size imgSize, Size winStride ) const
//	{
//		winStride.width = std::max(winStride.width, 1);
//		winStride.height = std::max(winStride.height, 1);
//		int border = largeSize/2 + smallSize/2;
//		return Size(std::max(imgSize.width - border*2 + winStride.width - 1, 0)/winStride.width,
//					std::max(imgSize.height - border*2 + winStride.height - 1, 0)/winStride.height);
//	}
//
//	// TODO: optimized with SSE2
//	void XSelfSimDescriptor::SSD(const Mat& img, Point pt, Mat& ssd) const
//	{
//		int x, y, dx, dy, r0 = largeSize/2, r1 = smallSize/2;
//		int step = img.step;
//		for( y = -r0; y <= r0; y++ )
//		{
//			float* sptr = ssd.ptr<float>(y+r0) + r0;
//			for( x = -r0; x <= r0; x++ )
//			{
//				int sum = 0;
//				const uchar* src0 = img.ptr<uchar>(y + pt.y - r1) + x + pt.x;
//				const uchar* src1 = img.ptr<uchar>(pt.y - r1) + pt.x;
//				for( dy = -r1; dy <= r1; dy++, src0 += step, src1 += step )
//					for( dx = -r1; dx <= r1; dx++ )
//					{
//						int t = src0[dx] - src1[dx];
//						sum += t*t;
//					}
//				sptr[x] = (float)sum;
//			}
//		}
//	}
//
//
//	void XSelfSimDescriptor::compute(const Mat& img, vector<float>& descriptors, Size winStride,
//									const vector<Point>& locations) const
//	{
//		CV_Assert( img.depth() == CV_8U );  
//
//		winStride.width = std::max(winStride.width, 1);
//		winStride.height = std::max(winStride.height, 1);
//		Size gridSize = getGridSize(img.size(), winStride);
//		int i, nwindows = locations.empty() ? gridSize.width*gridSize.height : (int)locations.size();
//		int border = largeSize/2 + smallSize/2;
//		int fsize = (int)getDescriptorSize();
//		vector<float> tempFeature(fsize+1);
//		descriptors.resize(fsize*nwindows + 1);
//		Mat ssd(largeSize, largeSize, CV_32F), mappingMask;
//		computeLogPolarMapping(mappingMask);
//
//	#if 0 //def _OPENMP
//		int nthreads = cvGetNumThreads();
//		#pragma omp parallel for num_threads(nthreads)
//	#endif
//		for( i = 0; i < nwindows; i++ )
//		{
//			Point pt;
//			float* feature0 = &descriptors[fsize*i];
//			float* feature = &tempFeature[0];
//			int x, y, j;
//
//			if( !locations.empty() )
//			{
//				pt = locations[i];
//				cout << "pt: " << pt.x << "x" << pt.y <<endl;
//				if( pt.x < border || pt.x >= img.cols - border ||
//					pt.y < border || pt.y >= img.rows - border )
//				{
//					for( j = 0; j < fsize; j++ )
//						feature0[j] = 0.f;
//					continue;
//				}
//			}
//			else
//				pt = Point((i % gridSize.width)*winStride.width + border,
//						   (i / gridSize.width)*winStride.height + border);
//
//			SSD(img, pt, ssd);
//			cout << "SSD: " << ssd.rows << " x "<<ssd.cols<<endl;
//			//print_mat<float>(ssd);
//			// Determine in the local neighborhood the largest difference and use for normalization
//			float var_noise = 1000.f;
//			for( y = -1; y <= 1 ; y++ )
//				for( x = -1 ; x <= 1 ; x++ )
//					var_noise = std::max(var_noise, ssd.at<float>(largeSize/2+y, largeSize/2+x));
//
//			for( j = 0; j <= fsize; j++ )
//				feature[j] = FLT_MAX;
//
//			// Derive feature vector before exp(-x) computation
//			// Idea: for all  x,a >= 0, a=const.   we have:
//			//       max [ exp( -x / a) ] = exp ( -min(x) / a )
//			// Thus, determine min(ssd) and store in feature[...]
//    		for( y = 0; y < ssd.rows; y++ )
//			{
//				const schar *mappingMaskPtr = mappingMask.ptr<schar>(y);
//				const float *ssdPtr = ssd.ptr<float>(y);
//				for( x = 0 ; x < ssd.cols; x++ )
//				{
//					int index = mappingMaskPtr[x];
//					feature[index] = std::max(feature[index], ssdPtr[x]);
//					cout <<"feature["<<index<<"]: " << feature[index]<< endl;
//				}
//			}
//
//			var_noise = -1.f/var_noise;
//			cout << "var_noise: " << var_noise<<endl;
//    		for( j = 0; j < fsize; j++ )
//				feature0[j] = feature[j]*var_noise;
//			Mat _f(1, fsize, CV_32F, feature0);
//			//print_mat<float>(_f);
//			cv::exp(_f, _f);
//			cout << "_f: \n"; 
//			//print_mat<float>(_f);
//		}
//	}
//
//	void XSelfSimDescriptor::computeLogPolarMapping(Mat& mappingMask) const
//	{
//		mappingMask.create(largeSize, largeSize, CV_8S);
//
//		// What we want is
//		//		 log_m (radius) = numberOfDistanceBuckets
//		//	<==> log_10 (radius) / log_10 (m) = numberOfDistanceBuckets
//		//	<==> log_10 (radius) / numberOfDistanceBuckets = log_10 (m)
//		//	<==> m = 10 ^ log_10(m) = 10 ^ [log_10 (radius) / numberOfDistanceBuckets]
//		//
//		int radius = largeSize/2, angleBucketSize = 360 / numberOfAngles;
//		int fsize = (int)getDescriptorSize();
//		double inv_log10m = (double)numberOfDistanceBuckets/log10((double)radius);
//
//		for (int y=-radius ; y<=radius ; y++)
//		{
//			schar* mrow = mappingMask.ptr<schar>(y+radius);
//			for (int x=-radius ; x<=radius ; x++)
//			{
//				int index = fsize;
//				float dist = (float)std::sqrt((float)x*x + (float)y*y);
//				int distNo = dist > 0 ? cvRound(log10(dist)*inv_log10m) : 0;
//				if( startDistanceBucket <= distNo && distNo < numberOfDistanceBuckets )
//				{
//					float angle = std::atan2( (float)y, (float)x ) / (float)CV_PI * 180.0f;
//					if (angle < 0) angle += 360.0f;
//					int angleInt = (cvRound(angle) + angleBucketSize/2) % 360;
//					int angleIndex = angleInt / angleBucketSize;
//					index = (distNo-startDistanceBucket)*numberOfAngles + angleIndex;
//				}
//				mrow[x + radius] = saturate_cast<schar>(index);
//			}
//		}
//	}
//
//	int test_lss()
//	{
//		XSelfSimDescriptor ss;
//		int N = ss.getDescriptorSize();
//		vector<float> desc;
//		Mat img = imread("gazelle.bmp");
//		//imshow("t",img);
//		//waitKey();
//		vector<Point> locs;
//		
//		locs.push_back(Point(300,300));
//		locs.push_back(Point(400,400));
//		locs.push_back(Point(100,100));
//		locs.push_back(Point(330,330));
//		locs.push_back(Point(350,350));
//		
//		ss.compute( img, desc, Size(10,10), locs );
//		cout << "desc.size(): " << desc.size() << endl; 
//
//
//		Size grids = ss.getGridSize(img.size(),Size(10,10));
//		cout << "N: "<< N<<endl;
//		cout << "grids: " << grids.height << " x "<< grids.width << endl;
//		Mat_<float> descmat(5,N);
//		for( int i = 0 ; i < 5 ; ++i )
//		{
//			cout << "locs[i]: " << locs[i].x << ", " << locs[i].y << endl;
//			for(int j = 0 ; j < N ; ++j)
//				descmat(i,j) = desc[i*N+j];
//		}
//		print_mat<float>(descmat);
//		assert(countNonZero(descmat));
//		return 0;
//	}
//	
}